/*
This file is part of CanFestival, a library implementing CanOpen Stack.

Copyright (C): Edouard TISSERANT and Francis DUPIN

See COPYING file for copyrights details.

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#if defined(WIN32) && !defined(__CYGWIN__)
#include <windows.h>
#include "getopt.h"
void pause(void)
{
    system("PAUSE");
}
#else
#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <signal.h>
#endif

#include "canfestival.h"
#include "TestMasterMicroMod.h"
#include "TestMaster.h"
unsigned int slavenodeid = 0x40;


/*****************************************************************************/
void TestMaster_heartbeatError(CO_Data* d, UNS8 heartbeatID)
{
    eprintf("TestMaster_heartbeatError %d\n", heartbeatID);
}

/********************************************************
 * ConfigureSlaveNode is responsible to
 *  - setup master RPDO 1 to receive TPDO 1 from id 0x40
 *  - setup master TPDO 1 to send RPDO 1 to id 0x40
 ********************************************************/
void TestMaster_initialisation(CO_Data* d)
{
    UNS32 PDO1_COBID = 0x0180 + slavenodeid;
    UNS32 PDO2_COBID = 0x0200 + slavenodeid;
    UNS32 size = sizeof(UNS32);
    eprintf("TestMaster_initialisation\n");
    /*****************************************
     * Define RPDOs to match slave ID=0x40 TPDOs*
     *****************************************/
    writeLocalDict(&TestMaster_Data,  /*CO_Data* d*/
                   0x1400, /*UNS16 index*/
                   0x01, /*UNS8 subind*/
                   &PDO1_COBID, /*void * pSourceData,*/
                   &size, /* UNS8 * pExpectedSize*/
                   RW);  /* UNS8 checkAccess */
    /*****************************************
     * Define TPDOs to match slave ID=0x40 RPDOs*
     *****************************************/
    writeLocalDict(&TestMaster_Data,  /*CO_Data* d*/
                   0x1800, /*UNS16 index*/
                   0x01, /*UNS8 subind*/
                   &PDO2_COBID, /*void * pSourceData,*/
                   &size, /* UNS8 * pExpectedSize*/
                   RW);  /* UNS8 checkAccess */
}

static int init_step = 0;

/*Froward declaration*/
static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId);

/**/
static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
{
    UNS32 abortCode;
    if (getWriteResultNetworkDict(d, nodeId, &abortCode) != SDO_FINISHED)
    {
        eprintf("Master : Failed in initializing slave %2.2x, step %d, AbortCode :%4.4x \n", nodeId, init_step, abortCode);
    }
    /* Finalise last SDO transfer with this node */
    closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);
    ConfigureSlaveNode(d, nodeId);
}

/********************************************************
 * ConfigureSlaveNode is responsible to
 *  - setup slave TPDO 1 transmit time
 *  - setup slave TPDO 2 transmit time
 *  - setup slave Heartbeat Producer time
 *  - switch to operational mode
 *  - send NMT to slave
 ********************************************************
 * This an example of :
 * Network Dictionary Access (SDO) with Callback
 * Slave node state change request (NMT)
 ********************************************************
 * This is called first by TestMaster_preOperational
 * then it called again each time a SDO exchange is
 * finished.
 ********************************************************/
static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)
{
    UNS8 res;
    eprintf("Master : ConfigureSlaveNode %2.2x\n", nodeId);
    switch (++init_step)
    {
    case 1:
    {
        /*disable Slave's TPDO 1 */
        UNS32 TPDO_COBId = 0x80000180 + nodeId;
        eprintf("Master : disable slave %2.2x TPDO 1 \n", nodeId);
        res = writeNetworkDictCallBack(d,  /*CO_Data* d*/
                                       /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
                                       nodeId, /*UNS8 nodeId*/
                                       0x1800, /*UNS16 index*/
                                       0x01, /*UNS8 subindex*/
                                       4, /*UNS8 count*/
                                       0, /*UNS8 dataType*/
                                       &TPDO_COBId,/*void *data*/
                                       CheckSDOAndContinue, /*SDOCallback_t Callback*/
                                       0); /* use block mode */
    }
    break;
    case 2:
    {
        /*setup Slave's TPDO 1 to be transmitted on SYNC*/
        UNS8 Transmission_Type = 0x01;
        eprintf("Master : set slave %2.2x TPDO 1 transmit type\n", nodeId);
        res = writeNetworkDictCallBack(d,  /*CO_Data* d*/
                                       /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
                                       nodeId, /*UNS8 nodeId*/
                                       0x1800, /*UNS16 index*/
                                       0x02, /*UNS8 subindex*/
                                       1, /*UNS8 count*/
                                       0, /*UNS8 dataType*/
                                       &Transmission_Type,/*void *data*/
                                       CheckSDOAndContinue, /*SDOCallback_t Callback*/
                                       0); /* use block mode */
    }
    break;
    case 3:
    {
        /*re-enable Slave's TPDO 1 */
        UNS32 TPDO_COBId = 0x00000180 + nodeId;
        eprintf("Master : re-enable slave %2.2x TPDO 1\n", nodeId);
        res = writeNetworkDictCallBack(d,  /*CO_Data* d*/
                                       /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
                                       nodeId, /*UNS8 nodeId*/
                                       0x1800, /*UNS16 index*/
                                       0x01, /*UNS8 subindex*/
                                       4, /*UNS8 count*/
                                       0, /*UNS8 dataType*/
                                       &TPDO_COBId,/*void *data*/
                                       CheckSDOAndContinue, /*SDOCallback_t Callback*/
                                       0); /* use block mode */
    }
    break;
    case 4:
    {
        /*disable Slave's RPDO 1 */
        UNS32 TPDO_COBId = 0x80000200 + nodeId;
        eprintf("Master : disable slave %2.2x RPDO 1\n", nodeId);
        res = writeNetworkDictCallBack(d,  /*CO_Data* d*/
                                       /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
                                       nodeId, /*UNS8 nodeId*/
                                       0x1400, /*UNS16 index*/
                                       0x01, /*UNS8 subindex*/
                                       4, /*UNS8 count*/
                                       0, /*UNS8 dataType*/
                                       &TPDO_COBId,/*void *data*/
                                       CheckSDOAndContinue, /*SDOCallback_t Callback*/
                                       0); /* use block mode */
    }
    break;
    case 5:
    {
        UNS8 Transmission_Type = 0x01;
        eprintf("Master : set slave %2.2x RPDO 1 receive type\n", nodeId);
        res = writeNetworkDictCallBack(d,  /*CO_Data* d*/
                                       /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
                                       nodeId, /*UNS8 nodeId*/
                                       0x1400, /*UNS16 index*/
                                       0x02, /*UNS8 subindex*/
                                       1, /*UNS8 count*/
                                       0, /*UNS8 dataType*/
                                       &Transmission_Type,/*void *data*/
                                       CheckSDOAndContinue, /*SDOCallback_t Callback*/
                                       0); /* use block mode */
    }
    break;
    case 6:
    {
        /*re-enable Slave's RPDO 1 */
        UNS32 TPDO_COBId = 0x00000200 + nodeId;
        eprintf("Master : re-enable %2.2x RPDO 1\n", nodeId);
        res = writeNetworkDictCallBack(d,  /*CO_Data* d*/
                                       /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
                                       nodeId, /*UNS8 nodeId*/
                                       0x1400, /*UNS16 index*/
                                       0x01, /*UNS8 subindex*/
                                       4, /*UNS8 count*/
                                       0, /*UNS8 dataType*/
                                       &TPDO_COBId,/*void *data*/
                                       CheckSDOAndContinue, /*SDOCallback_t Callback*/
                                       0); /* use block mode */
    }
    break;
    case 7:
    {
        /*set the heartbeat Producer Time*/
        UNS16 Heartbeat_Producer_Time = 0x03E8;
        eprintf("Master : set slave %2.2x heartbeat producer time \n", nodeId);
        res = writeNetworkDictCallBack(d,  /*CO_Data* d*/
                                       /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
                                       nodeId, /*UNS8 nodeId*/
                                       0x1017, /*UNS16 index*/
                                       0x00, /*UNS8 subindex*/
                                       2, /*UNS8 count*/
                                       0, /*UNS8 dataType*/
                                       &Heartbeat_Producer_Time,/*void *data*/
                                       CheckSDOAndContinue, /*SDOCallback_t Callback*/
                                       0); /* use block mode */
    }
    break;
    case 8:
    {
        /*disable Slave's TPDO 2 */
        UNS32 TPDO_COBId = 0x80000280 + nodeId;
        eprintf("Master : disable slave %2.2x TPDO 2 \n", nodeId);
        res = writeNetworkDictCallBack(d,  /*CO_Data* d*/
                                       /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
                                       nodeId, /*UNS8 nodeId*/
                                       0x1801, /*UNS16 index*/
                                       0x01, /*UNS8 subindex*/
                                       4, /*UNS8 count*/
                                       0, /*UNS8 dataType*/
                                       &TPDO_COBId,/*void *data*/
                                       CheckSDOAndContinue, /*SDOCallback_t Callback*/
                                       0); /* use block mode */
    }
    break;
    case 9:
    {
        /*disable Slave's TPDO 3 */
        UNS32 TPDO_COBId = 0x80000380 + nodeId;
        eprintf("Master : disable slave %2.2x TPDO 3 \n", nodeId);
        res = writeNetworkDictCallBack(d,  /*CO_Data* d*/
                                       /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
                                       nodeId, /*UNS8 nodeId*/
                                       0x1802, /*UNS16 index*/
                                       0x01, /*UNS8 subindex*/
                                       4, /*UNS8 count*/
                                       0, /*UNS8 dataType*/
                                       &TPDO_COBId,/*void *data*/
                                       CheckSDOAndContinue, /*SDOCallback_t Callback*/
                                       0); /* use block mode */
    }
    break;
    case 10:
    {
        /*disable Slave's TPDO 4 */
        UNS32 TPDO_COBId = 0x80000480 + nodeId;
        eprintf("Master : disable slave %2.2x TPDO 4 \n", nodeId);
        res = writeNetworkDictCallBack(d,  /*CO_Data* d*/
                                       /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
                                       nodeId, /*UNS8 nodeId*/
                                       0x1803, /*UNS16 index*/
                                       0x01, /*UNS8 subindex*/
                                       4, /*UNS8 count*/
                                       0, /*UNS8 dataType*/
                                       &TPDO_COBId,/*void *data*/
                                       CheckSDOAndContinue, /*SDOCallback_t Callback*/
                                       0); /* use block mode */
    }
    break;
    case 11:
        /* Put the master in operational mode */
        setState(d, Operational);
        /* Ask slave node to go in operational mode */
        masterSendNMTstateChange(d, nodeId, NMT_Start_Node);
    }
}

void TestMaster_preOperational(CO_Data* d)
{
    eprintf("TestMaster_preOperational\n");
    ConfigureSlaveNode(&TestMaster_Data, slavenodeid);
}

void TestMaster_operational(CO_Data* d)
{
    eprintf("TestMaster_operational\n");
}

void TestMaster_stopped(CO_Data* d)
{
    eprintf("TestMaster_stopped\n");
}

void TestMaster_post_sync(CO_Data* d)
{
    DO++;
    eprintf("MicroMod Digital Out: %2.2x\n", DO);
    eprintf("MicroMod Digital In (by bit): DI1: %2.2x DI2: %2.2x DI3: %2.2x DI4: %2.2x DI5: %2.2x DI6: %2.2x DI7: %2.2x DI8: %2.2x\n", DI1, DI2, DI3, DI4, DI5, DI6, DI7, DI8);
}

void TestMaster_post_TPDO(CO_Data* d)
{
    //  eprintf("TestMaster_post_TPDO\n");
}

//s_BOARD SlaveBoard = {"0", "500K"};
s_BOARD MasterBoard = {"0", "125K"};

#if !defined(WIN32) || defined(__CYGWIN__)
void catch_signal(int sig)
{
    signal(SIGTERM, catch_signal);
    signal(SIGINT, catch_signal);
    eprintf("Got Signal %d\n", sig);
}
#endif

void help(void)
{
    printf("**************************************************************\n");
    printf("*  TestMasterMicroMod                                        *\n");
    printf("*                                                            *\n");
    printf("*  A simple example for PC.                                  *\n");
    printf("*  A CanOpen master that control a MicroMod module:          *\n");
    printf("*  - setup module TPDO 1 transmit type                       *\n");
    printf("*  - setup module RPDO 1 transmit type                       *\n");
    printf("*  - setup module hearbeatbeat period                        *\n");
    printf("*  - disable others TPDOs                                    *\n");
    printf("*  - set state to operational                                *\n");
    printf("*  - send periodic SYNC                                      *\n");
    printf("*  - send periodic RPDO 1 to Micromod (digital output)       *\n");
    printf("*  - listen Micromod's TPDO 1 (digital input)                *\n");
    printf("*  - Mapping RPDO 1 bit per bit (digital input)              *\n");
    printf("*                                                            *\n");
    printf("*   Usage:                                                   *\n");
    printf("*   ./TestMasterMicroMod  [OPTIONS]                          *\n");
    printf("*                                                            *\n");
    printf("*   OPTIONS:                                                 *\n");
    printf("*     -l : Can library [\"libcanfestival_can_virtual.so\"]     *\n");
    printf("*                                                            *\n");
    printf("*    Slave:                                                  *\n");
    printf("*     -i : Slave Node id format [0x01 , 0x7F]                *\n");
    printf("*                                                            *\n");
    printf("*    Master:                                                 *\n");
    printf("*     -m : bus name [\"1\"]                                    *\n");
    printf("*     -M : 1M,500K,250K,125K,100K,50K,20K,10K                *\n");
    printf("*                                                            *\n");
    printf("**************************************************************\n");
}

/***************************  INIT  *****************************************/
void InitNodes(CO_Data* d, UNS32 id)
{
    /****************************** INITIALISATION MASTER *******************************/
    if (MasterBoard.baudrate)
    {
        /* Defining the node Id */
        setNodeId(&TestMaster_Data, 0x01);
        /* init */
        setState(&TestMaster_Data, Initialisation);
    }
}

/***************************  EXIT  *****************************************/
void Exit(CO_Data* d, UNS32 id)
{
    if (strcmp(MasterBoard.baudrate, "none"))
    {
        masterSendNMTstateChange(&TestMaster_Data, 0x02, NMT_Reset_Node);
        //Stop master
        setState(&TestMaster_Data, Stopped);
    }
}
/****************************************************************************/
/***************************  MAIN  *****************************************/
/****************************************************************************/
int main(int argc, char** argv)
{
    int c;
    extern char* optarg;
    char* LibraryPath = "libcanfestival_can_virtual.so";
    char* snodeid;
    while ((c = getopt(argc, argv, "-m:s:M:S:l:i:")) != EOF)
    {
        switch (c)
        {
        case 'm' :
            if (optarg[0] == 0)
            {
                help();
                exit(1);
            }
            MasterBoard.busname = optarg;
            break;
        case 'M' :
            if (optarg[0] == 0)
            {
                help();
                exit(1);
            }
            MasterBoard.baudrate = optarg;
            break;
        case 'l' :
            if (optarg[0] == 0)
            {
                help();
                exit(1);
            }
            LibraryPath = optarg;
            break;
        case 'i' :
            if (optarg[0] == 0)
            {
                help();
                exit(1);
            }
            snodeid = optarg;
            sscanf(snodeid, "%x", &slavenodeid);
            break;
        default:
            help();
            exit(1);
        }
    }
#if !defined(WIN32) || defined(__CYGWIN__)
    /* install signal handler for manual break */
    signal(SIGTERM, catch_signal);
    signal(SIGINT, catch_signal);
    TimerInit();
#endif
#ifndef NOT_USE_DYNAMIC_LOADING
    LoadCanDriver(LibraryPath);
#endif
    TestMaster_Data.heartbeatError = TestMaster_heartbeatError;
    TestMaster_Data.initialisation = TestMaster_initialisation;
    TestMaster_Data.preOperational = TestMaster_preOperational;
    TestMaster_Data.operational = TestMaster_operational;
    TestMaster_Data.stopped = TestMaster_stopped;
    TestMaster_Data.post_sync = TestMaster_post_sync;
    TestMaster_Data.post_TPDO = TestMaster_post_TPDO;
    if (!canOpen(&MasterBoard, &TestMaster_Data))
    {
        eprintf("Cannot open Master Board\n");
        goto fail_master;
    }
    // Start timer thread
    StartTimerLoop(&InitNodes);
    // wait Ctrl-C
    pause();
    eprintf("Finishing.\n");
    // Stop timer thread
    StopTimerLoop(&Exit);
fail_master:
    if (MasterBoard.baudrate)
    {
        canClose(&TestMaster_Data);
    }
    TimerCleanup();
    return 0;
}


